/**
 ****************************************************************
 *
 * description : 带限制的电机控制模块
 * version     : V1.1
 * Revision    : V1.1 将以前的MSG与InSpeed,整理成统一的函数
 * 			     V1.0 增加带速度控制的功能
 * Author      : DJM <d.jiaming@qq.com>
 *
 ****************************************************************
 */

#include "Module_MC_WithLimited.h"
//#include "bsp.h"

static void MCWL_MSG_Exec(MCWL_HandleTypeDef *phmotor);
static void MCWL_ReturnToZero(MCWL_HandleTypeDef *phmotor);
static __inline void MCWL_SetTimeout(MCWL_HandleTypeDef *phmotor,uint32_t timeout);

/**
 * [MCWL_Init 相关初始化]
 * @param  phmotor 模块的句柄
 * @return         初始化成功返回MCWL_ERROR_OK,否则返回MCWL_ERROR_ERROR
 */
MCWL_ERRORTypeDef MCWL_Init(MCWL_HandleTypeDef *phmotor)
{
	if(!phmotor->Init.MotorOperate||!phmotor->Init.GetMotorPosition||!phmotor->Init.MotorStop||!phmotor->Init.MotorIdle)
	{
		return MCWL_ERROR_ERROR;
	}
	phmotor->state = MCWL_READY;
	phmotor->CurrentLevel = 0;
	phmotor->ZeroInit.state = MCWL_ZeroInit_Busy;
	return MCWL_ERROR_OK;
}

/**
 * [MCWL_Exec MCWL模块定时执行函数]
 * @param  phmotor 模块句柄
 * @return         [MCWL_ERRORTypeDef]
 */
MCWL_ERRORTypeDef MCWL_Exec(MCWL_HandleTypeDef *phmotor)
{
	MCWL_ReturnToZero(phmotor);//归零动作执行函数
	MCWL_MSG_Exec(phmotor);//动作指令执行函数
	if(phmotor->Init.DangerousZone != 0)//为0时不启用，DangerousZone+MinLimit&MaxLimit不能超过int32的最小和最大数
	{
		if((phmotor->Init.GetMotorPosition() < phmotor->Init.MinLimit- phmotor->Init.DangerousZone)|| \
			(phmotor->Init.GetMotorPosition() > phmotor->Init.MaxLimit + phmotor->Init.DangerousZone))
		{
			if(phmotor->DZ_Timeout == 0)//进入危险区域
				phmotor->DZ_Timeout = phmotor->Init.DangerousZoneTimeout + Gettime();//
			if(Gettime() > phmotor->DZ_Timeout)//超时
			{
				phmotor->state = MCWL_LOCKED;//状态锁定,只有重启才能重新使用电机控制
				phmotor->Init.MotorStop();//电机失能
				return MCWL_ERROR_ERROR;
			}
		}
		else
			phmotor->DZ_Timeout = 0;//离开危险区域
	}
	if(Gettime() > phmotor->Timeout && phmotor->Timeout != 0)//超时保护执行部分
	{
		int32_t Pos;
		Pos = phmotor->Init.GetMotorPosition();
		MCWL_Constrain(Pos,phmotor->Init.MinLimit,phmotor->Init.MaxLimit)
		phmotor->Init.MotorOperate(Pos);
		phmotor->state = MCWL_READY;
		phmotor->CurrentLevel = 0;//等级归零
		phmotor->Timeout = 0;//超时保护失能
		phmotor->MSG.state = MCWL_ON;//动作指令执行函数失能
		return MCWL_ERROR_TIMEOUT;
	}
	if(phmotor->state == MCWL_BUSY)
	{
		MCWL_Constrain(phmotor->TargetPosition,phmotor->Init.MinLimit,phmotor->Init.MaxLimit)
		phmotor->Init.MotorOperate(phmotor->TargetPosition);

		//判断是否到达目的位置
		if((phmotor->Dir == MCWL_OpposeZero) ? (phmotor->TargetPosition > phmotor->Init.GetMotorPosition()):(phmotor->TargetPosition < phmotor->Init.GetMotorPosition()))
//		if(MCWL_ABS(phmotor->Init.GetMotorPosition() - phmotor->TargetPosition) < phmotor->Init.DeadZone )
		{
			phmotor->state = MCWL_READY;
			phmotor->CurrentLevel = 0;//等级归零
			return MCWL_ERROR_OK;
		}
		return MCWL_ERROR_BUSY;
	}
	else if(phmotor->state == MCWL_READY)
	{
		phmotor->Init.MotorIdle();//执行空闲函数
		return MCWL_ERROR_READY;
	}
	return MCWL_ERROR_ERROR;
}




/**
 * [MCWL_ReturnToZero 归零动作执行函数]
 * @param phmotor [模块句柄]
 */
static void MCWL_ReturnToZero(MCWL_HandleTypeDef *phmotor)
{
	switch(phmotor->ZeroInit.state)
	{
		case MCWL_ZeroInit_Busy:
								if(phmotor->ZeroInit.ToZeroStatus() == MCWL_ON)
								{
									phmotor->ZeroInit.state = MCWL_ZeroInit_1stIsOn;//开始时处于零点以下
									MCWL_SetTimeout(phmotor,phmotor->ZeroInit.TimeoutOpposeZero);//使能超时保护

								}
								else if(phmotor->ZeroInit.ToZeroStatus() == MCWL_OFF)
								{
									phmotor->ZeroInit.state = MCWL_ZeroInit_1stIsOff;//开始时处于零点以上
									MCWL_SetTimeout(phmotor,phmotor->ZeroInit.TimeoutToZero);//使能超时保护
								}
								break;
		case MCWL_ZeroInit_1stIsOn:
								if(phmotor->ZeroInit.ToZeroStatus() == MCWL_ON && phmotor->Timeout)
								{
									MCWL_MSG_Set(phmotor,2147483647,phmotor->ZeroInit.SpeedOpposeZero,0xFFFFFFFF);
								}
								else
								{
									phmotor->MSG.state = MCWL_ON;
									phmotor->ZeroInit.state = MCWL_ZeroInit_1stIsOff;
									MCWL_SetTimeout(phmotor,phmotor->ZeroInit.TimeoutToZero);//使能超时保护
								}
								break;
		case MCWL_ZeroInit_1stIsOff:
								if(phmotor->ZeroInit.ToZeroStatus() == MCWL_OFF && phmotor->Timeout)
								{
									MCWL_MSG_Set(phmotor,-2147483647,phmotor->ZeroInit.SpeedToZero,0xFFFFFFFF);
								}
								else
								{
									phmotor->MSG.state = MCWL_ON;
									if(!phmotor->Timeout)//超时回退
									{
										phmotor->ZeroOffset = phmotor->Init.GetMotorPosition()  + phmotor->ZeroInit.BackwardToZero;
									}
									else
										phmotor->ZeroOffset = phmotor->Init.GetMotorPosition();
									
									phmotor->Init.MinLimit = phmotor->Init.MinLimitReferToZero + phmotor->ZeroOffset;
									phmotor->Init.MaxLimit = phmotor->Init.MaxLimitReferToZero + phmotor->ZeroOffset;
									phmotor->Init.MotorOperate(phmotor->ZeroOffset);
									phmotor->ZeroInit.state = MCWL_ZeroInit_Ready;
								}
								break;
#if OpposeToTest
//		case MCWL_ZeroInit_ReverseToLimit:
//								phmotor->ZeroInit.LimitMSG.Level = 0xFFFFFFFF;
//								if(phmotor->ZeroInit.ToOpposeZeroStatus() == MCWL_OFF)
//								{
//									MCWL_MSG_Set(&phmotor->ZeroInit.LimitMSG,phmotor->Init.GetMotorPosition() + phmotor->ZeroInit.Speed,phmotor->ZeroInit.Timeout);
//									MCWL_MSG_Exec(phmotor,&phmotor->ZeroInit.LimitMSG);
//								}
//								else
//								{
//									phmotor->Init.MaxLimit = phmotor->TargetPosition;
//									MCWL_MSG_Set(&phmotor->ZeroInit.LimitMSG,(phmotor->Init.MinLimit + phmotor->Init.MaxLimit)/2,phmotor->ZeroInit.Timeout);
//									MCWL_MSG_Exec(phmotor,&phmotor->ZeroInit.LimitMSG);
//									phmotor->ZeroInit.state = MCWL_ZeroInit_Ready;
//								}
//								break;
#endif
		default:break;
	}
}


/**
 * [MCWL_MSG_Exec 消息执行函数,利用之前消息机制重构的控制速度的电机动作]
 * @param phmotor [模块句柄]
 */
static void MCWL_MSG_Exec(MCWL_HandleTypeDef *phmotor)
{
	int32_t Position_t;
	if(phmotor->MSG.state == MCWL_OFF)
	{
		if((phmotor->MSG.Dir == MCWL_OpposeZero) ? \
			 (phmotor->MSG.Position > phmotor->Init.GetMotorPosition() + phmotor->MSG.Speed) \
			:(phmotor->MSG.Position < phmotor->Init.GetMotorPosition() + phmotor->MSG.Speed) )
		{
			Position_t = phmotor->Init.GetMotorPosition() + phmotor->MSG.Speed;
		}
		else
		{
			Position_t = phmotor->MSG.Position;
			phmotor->MSG.state = MCWL_ON;//下次不执行
		}

		if(phmotor->state != MCWL_RESET || phmotor->state != MCWL_LOCKED)
		{
			if(phmotor->MSG.Level >= phmotor->CurrentLevel)
			{
				phmotor->state = MCWL_READY;
			}
			if(phmotor->state == MCWL_READY)
			{
				if(phmotor->TargetPosition > phmotor->Init.GetMotorPosition())
					phmotor->Dir = MCWL_OpposeZero;
				else
					phmotor->Dir = MCWL_ToZero;

				phmotor->CurrentLevel = phmotor->MSG.Level;
				phmotor->TargetPosition = Position_t;
				phmotor->state = MCWL_BUSY;
			}
		}
	}
}

/**
 * [MCWL_SetTimeout 设置超时保护]
 * @param phmotor [模块句柄]
 * @param timeout [超时时间]
 */
static __inline void MCWL_SetTimeout(MCWL_HandleTypeDef *phmotor,uint32_t timeout)
{
	phmotor->Timeout = timeout + Gettime();
}

/**
 * [MCWL_MSG_Set 模块动作设置函数(不带超时保护)]
 * @param phmotor  [模块句柄]
 * @param position [目标位置]
 * @param speed    [执行速度]
 * @param level    [消息等级]
 */
void MCWL_MSG_Set(MCWL_HandleTypeDef *phmotor,int32_t position,int32_t speed,uint32_t level)
{
	if(!speed)//速度为0时，将速度强制设为0xFFFF(较大)
		speed = 0xFFFF;
	phmotor->MSG.Level = level;
	phmotor->MSG.Position = position;

	if(position > phmotor->Init.GetMotorPosition())
	{
		phmotor->MSG.Speed = speed;
		phmotor->MSG.Dir = MCWL_OpposeZero;
	}
	else
	{
		phmotor->MSG.Speed = -speed;
		phmotor->MSG.Dir = MCWL_ToZero;
	}
	phmotor->MSG.state = MCWL_OFF;
}
/**
 * [MCWL_MSG_SetTimeout 模块动作设置函数(带超时保护)]
 * @param phmotor  [模块句柄]
 * @param position [目标位置]
 * @param speed    [执行速度]
 * @param level    [消息等级]
 * @param timeout  [超时保护]
 */
void MCWL_MSG_SetTimeout(MCWL_HandleTypeDef *phmotor,int32_t position,int32_t speed,uint32_t level,uint32_t timeout)
{
	MCWL_MSG_Set(phmotor,position,speed,level);
	MCWL_SetTimeout(phmotor,timeout);
}

/**
 * [MCWL_Calculate 这是一个控制动作的demo,以归零后的零点phmotor->ZeroOffset为起点控制]
 * @param phmotor [模块句柄]
 * @param pos     [目标位置]
 * @param speed   [执行速度]
 */
void MCWL_Calculate(MCWL_HandleTypeDef *phmotor,int32_t pos,int32_t speed)
{
	MCWL_MSG_Set(phmotor,pos + phmotor->ZeroOffset ,speed,0xffff);
}

/**
 * [Set_Encoder 更新码盘数据]
 * @param pEncoder [相关码盘的句柄]
 * @param cEncoder [码盘的数值]
 */
void Set_Encoder(EncoderStuct *pEncoder,int32_t cEncoder)
{
	pEncoder->this_coder = cEncoder;

	if(pEncoder->this_coder - pEncoder->last_coder > 4096)
	{
		pEncoder->turn_cnt--;
	}
	else if(pEncoder->this_coder - pEncoder->last_coder < -4096)
	{
		pEncoder->turn_cnt++;
	}
	pEncoder->last_coder = pEncoder->this_coder;

	pEncoder->Position = pEncoder->this_coder + pEncoder->turn_cnt * 8192;
	pEncoder->Speed = pEncoder->Position - pEncoder->last_position;
	pEncoder->last_position = pEncoder->Position;
}
